/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_C3DREADER_H_
#define __MMM_C3DREADER_H_


#include "../MMMCore.h"
#include "../MMMImportExport.h"
#include "MarkerData.h"

#include <string>
#include <map>
#include <Eigen/Core>
#include <fstream>
#include <iostream>

namespace MMM
{

/*! \brief Stores motion capture data at certain times grouping them by their motion prefix in the c3d file. */
class PrefixMarkerData {
public:
    PrefixMarkerData()
    {
    }

    /*! Checks if there is marker data with a specific prefix.
        @param prefix The marker data prefix which can be an empty string. */
    bool hasMarkerData(const std::string &prefix) {
        return prefixMarkerData.find(prefix) != prefixMarkerData.end();
    }

    /*! Gets all marker data matching the given prefix.
        @param prefix The marker data prefix which can be an empty string.
        @return Timesteps mapped on MarkerData. */
    std::map<float, MarkerDataPtr> getMarkerData(const std::string &prefix) {
        return prefixMarkerData[prefix];
    }

    /*! Method for adding marker data. */
    void addMarkerData(float timestep, const Eigen::Vector3f &marker_data, const std::string &marker_name = std::string(), const std::string &marker_prefix = std::string()) {
        MarkerDataPtr m(new MarkerData());
        if (prefixMarkerData.find(marker_prefix) != prefixMarkerData.end()) {
            MarkerDataPtr markerData = prefixMarkerData[marker_prefix][timestep];
            if (markerData) m = markerData;
            else prefixMarkerData[marker_prefix][timestep] = m;
        }
        else {
            prefixMarkerData[marker_prefix][timestep] = m;
            prefixes.push_back(marker_prefix);
        }

        m->addMarkerData(marker_data, marker_name);
    }

    /*! Get all marker data prefixes */
    std::vector<std::string> getPrefixes() {
        return prefixes;
    }

private:
    std::vector<std::string> prefixes;
    std::map<std::string, std::map<float, MarkerDataPtr> > prefixMarkerData;
};

typedef boost::shared_ptr<PrefixMarkerData> PrefixMarkerDataPtr;

/*! \brief This reader parses a Vicon C3D data file and creates a PrefixMarkerData object.*/
class MMM_IMPORT_EXPORT MotionReaderC3D {

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	MotionReaderC3D();

	virtual ~MotionReaderC3D();

    /*! Load marker data from c3d document.
        @param filename Path to c3d document.*/
    PrefixMarkerDataPtr loadC3D(const std::string filename);
	
protected:

    std::vector<Eigen::Vector3f> readMarkerData(unsigned short marker_num);

    int getNumFrames();

	int getNumMarkers();

	//header properties
	unsigned short num_channels;
	unsigned short first_field;
	unsigned short last_field;
	float scale_factor;
	unsigned short start_record_num;
	unsigned short frames_per_field;
	unsigned char ptype;

	long getFilePositionGroup(const char* group);
	long getFilePositionParams(long group_pos, const char* parameters);

    std::vector<std::string> Read_C3D_Marker_Labels();

	void Read_C3D_Header();

    std::vector<Eigen::Vector3f> Read_C3D_Marker(unsigned short marker_num);

	static void ConvertFloatToDec(float f, char* bytes);
	static float ConvertDecToFloat(char bytes[4]);

	void RPF(int *offset, char *type, char *dim);
	float convertFloat(char mem_float[4]);

	//file properties
	std::ifstream infile;
	std::string filename;

	//header properties
	unsigned short num_markers;
	float video_rate;

private:
    PrefixMarkerDataPtr marker;
};

typedef boost::shared_ptr<MotionReaderC3D> MotionReaderC3DPtr;

}

#endif /* C3DReader_H_ */
